﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.IO;
using System.Diagnostics;
using System.IO.Ports;

namespace R10App
{
    public partial class MainForm : Form
    {
        static uint canHandle;
        Thread RXThread;
        bool RXActive;
        const int IOBaseAddr = 0x0010000;
        const int IMUBaseAddr = 0x0030000;
        const int XBEEBaseAddr = 0x0040000;
        private Waveform waveForm1 = new Waveform();
        private Waveform waveForm2 = new Waveform();

        private JoystickHandler joy = new JoystickHandler();

        // data
        SDataStructure DataStructure = new SDataStructure();

        double GlobalRollRef = 0;

        double GRollOffset = 0;
        double GPitchOffset = 0;

        Bitmap bitMapMap;
        Bitmap offscreenSurf = new Bitmap(400, 260);

        public struct SDataStructure
        {
            public int ReceivedPackets;
            public int ReceiveErrors;
            public int RXThreadTicks;

            public int TransmitPackets;
            public int TransmitErrors;

            // data
            // Sensors - I/O Board
            public int ModeActual;
            public int Fail;
            public float Voltage;
            public float PWMOut1;
            public float PWMOut2;
            public float PWMOut3;
            public float PWMOut4;
            public float Sonar;
            public float IRNear;
            public float IRFar;
            public float LoopTime;
            public float LoopTimeMAX;
            public double TotalTime;

            public float MagX;
            public float MagY;
            public float MagZ;
            public float MagYaw;

            // IMU Board
            public float GyroX;
            public float GyroY;
            public float GyroZ;
            public float Roll;
            public float Pitch;
            public float Yaw;
            public float AbsPressure;
            public float DiffPressure;
            public float IMUTemperature;
            public double GPSLatitude;
            public double GPSLongitude;
            public float GPSAltitude;
            public float GPSSatNr;
            public float velX;
            public float velY;


            // Other Data
            public float AltitudeF;
            public float VertSpeed;

        };

        public MainForm()
        {
            InitializeComponent();

            string[] ports = SerialPort.GetPortNames();
            comboBoxPorts.Items.AddRange(ports);
            if (comboBoxPorts.Items.Count > 0) comboBoxPorts.SelectedIndex = 0;
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            toolStripStatusLabelCANAdapter.Text = "No adapters";

            // find adapters
            StringBuilder buf = new StringBuilder(32);
            int cnt;

            // Fill the listbox with data
            cnt = LAWICEL.canusb_getFirstAdapter(buf, 32);
            if (cnt > 0) toolStripStatusLabelCANAdapter.Text = "Adapter found: " + buf.ToString();

            joy.Init(this.Handle);

            // wavefrm init
            waveForm1.Init(30, 30, Color.Green, Color.LightSkyBlue); // Pitch/Roll
            waveForm2.Init(20, 20, Color.Yellow, Color.Red); // Gyro

            bitMapMap = (Bitmap)Bitmap.FromFile("mapa.bmp");
        } 

        private void buttonOpen_Click(object sender, EventArgs e)
        {
            canHandle = LAWICEL.canusb_Open(IntPtr.Zero,
                                        LAWICEL.CAN_BAUD_1M,
                                        LAWICEL.CANUSB_ACCEPTANCE_CODE_ALL,
                                        LAWICEL.CANUSB_ACCEPTANCE_MASK_ALL,
                                        0);

            if (canHandle == 0) MessageBox.Show("Open Failed");
            else
            {
                // OK
                buttonOpen.Enabled = false;
                buttonClose.Enabled = true;

                // init structures
                DataStructure = new SDataStructure();

                // create receive thread
                RXActive = true;
                RXThread = new Thread(new ThreadStart(RXThreadProc));
                //RXThread.Priority = ThreadPriority.AboveNormal;
                RXThread.IsBackground = true;
                RXThread.Start();

                timerGUI.Enabled = true;
            }
        }

        private void buttonClose_Click(object sender, EventArgs e)
        {
            timerGUI.Enabled = false;

            // Close the CANUSB
            int res = LAWICEL.canusb_Close(canHandle);
            if (LAWICEL.ERROR_CANUSB_OK == res)
            {
                // OK
                buttonOpen.Enabled = true;
                buttonClose.Enabled = false;
                RXActive = false;
            }
            else
            {
                MessageBox.Show("Close Failed");
            }
        }

        void RXThreadProc()
        {
            // open log file
            StreamWriter logFile = File.CreateText(Application.StartupPath + "\\LogFile.txt");
            //logFile.WriteLine("TimeSec Voltage PWM1 PWM2");
            //logFile.WriteLine("TimeSec Omega Yaw MagX MagY MagZ");
            logFile.WriteLine("TimeSec Voltage PWM1 Yaw Pitch Roll GyroX GyroY GyroZ MagX MagY MagZ");
            Stopwatch timer = new Stopwatch();
            timer.Start();

            while (RXActive)
            {
                // Read one CAN message
                LAWICEL.CANMsg msg = new LAWICEL.CANMsg();

                int rv = LAWICEL.canusb_Read(canHandle, out msg);
                if (LAWICEL.ERROR_CANUSB_OK == rv)
                {
                    // data OK, convert to float
                    byte[] bytes = BitConverter.GetBytes(msg.data);
                    float data = BitConverter.ToSingle(bytes, 0);
                    double dataDbl = BitConverter.ToDouble(bytes, 0);

                    lock (this)
                    {
                        // IOBoard
                        uint baseAddr = IOBaseAddr;                        
                        if (msg.id == 0x311 + baseAddr) DataStructure.Voltage = data;
                        if (msg.id == 0x120 + baseAddr) DataStructure.PWMOut1 = data;
                        if (msg.id == 0x121 + baseAddr) DataStructure.PWMOut2 = data;
                        if (msg.id == 0x122 + baseAddr) DataStructure.PWMOut3 = data;
                        if (msg.id == 0x123 + baseAddr) DataStructure.PWMOut4 = data;
                        if (msg.id == 0x300 + baseAddr) DataStructure.Sonar = data / 100; // [m]
                        if (msg.id == 0x301 + baseAddr) DataStructure.IRNear = data / 100;
                        if (msg.id == 0x302 + baseAddr) DataStructure.IRFar = data / 100;

                        // IMU Board
                        baseAddr = IMUBaseAddr;
                        if (msg.id == 0x300 + baseAddr) DataStructure.GyroX = data;
                        if (msg.id == 0x301 + baseAddr) DataStructure.GyroY = data;
                        if (msg.id == 0x302 + baseAddr) DataStructure.GyroZ = data;
                        if (msg.id == 0x310 + baseAddr) DataStructure.Roll = data;
                        if (msg.id == 0x311 + baseAddr) DataStructure.Pitch = data;
                        if (msg.id == 0x312 + baseAddr) DataStructure.Yaw = data;
                        if (msg.id == 0x320 + baseAddr) DataStructure.AbsPressure = data;
                        if (msg.id == 0x321 + baseAddr) DataStructure.DiffPressure = data;
                        if (msg.id == 0x322 + baseAddr) DataStructure.IMUTemperature = data;
                        if (msg.id == 0x330 + baseAddr) DataStructure.GPSLongitude = dataDbl;
                        if (msg.id == 0x331 + baseAddr) DataStructure.GPSLatitude = dataDbl;
                        if (msg.id == 0x332 + baseAddr) DataStructure.GPSAltitude = data;
                        if (msg.id == 0x333 + baseAddr) DataStructure.GPSSatNr = data;
                        if (msg.id == 0x335 + baseAddr) DataStructure.velX = data;
                        if (msg.id == 0x336 + baseAddr) DataStructure.velY++;// = data;

                        if (msg.id == 0x306 + baseAddr) DataStructure.MagX = data;
                        if (msg.id == 0x307 + baseAddr) DataStructure.MagY = data;
                        if (msg.id == 0x308 + baseAddr) DataStructure.MagZ = data;

                        // XBee Board
                        baseAddr = XBEEBaseAddr;
                        if (msg.id == 0x400 + baseAddr) DataStructure.ModeActual = (int)data;
                        if (msg.id == 0x401 + baseAddr) DataStructure.Fail = (int)data;
                        if (msg.id == 0x350 + baseAddr) DataStructure.LoopTime = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.TotalTime = dataDbl;
                        if (msg.id == 0x352 + baseAddr) DataStructure.LoopTimeMAX = data;

                        DataStructure.ReceivedPackets++;
                    }

                    // log data
                    double currentMS = (double)timer.ElapsedTicks/(double)Stopwatch.Frequency * 1000.0;
                    // Log on Yaw 
                    if (msg.id == 0x312 + IMUBaseAddr)
                    {
                        //TimeSec Voltage PWM1 Yaw Pitch Roll GyroX GyroY GyroZ MagX MagY MagZ
                        string line = string.Format("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11}", currentMS / 1000, DataStructure.Voltage, DataStructure.PWMOut1, DataStructure.Yaw, DataStructure.Pitch, DataStructure.Roll, DataStructure.GyroX, DataStructure.GyroY, DataStructure.GyroZ, DataStructure.MagX, DataStructure.MagY, DataStructure.MagZ);
                        logFile.WriteLine(line);
                        //string line = string.Format("{0} {1} {2} {3} {4} {5} {6} {7} {8}", currentMS / 1000, DataStructure.Voltage, DataStructure.PWMOut1, DataStructure.PWMOut2, DataStructure.Current1, DataStructure.Power1, DataStructure.Force1FLT, DataStructure.Moment1FLT, DataStructure.SpeedRPM1);
                        //logFile.WriteLine(line);
                        // MAG Save
                        //string line = string.Format("{0} {1} {2} {3} {4} {5}", currentMS / 1000, DataStructure.OmegaZ, DataStructure.Yaw, DataStructure.MagX, DataStructure.MagY, DataStructure.MagZ);
                        //logFile.WriteLine(line);

                        // send to waveforms
                        SWFData[] newData = new SWFData[1];
                        newData[0].D1 = GlobalRollRef * 180 / Math.PI; // deg
                        newData[0].D2 = DataStructure.Roll * 180 / Math.PI;
                        waveForm1.AddPoints(newData);
                        newData[0].D1 = DataStructure.GyroX * 180 / Math.PI;
                        newData[0].D2 = DataStructure.GyroY * 180 / Math.PI;
                        waveForm2.AddPoints(newData); 
                    }
                }
                else if (LAWICEL.ERROR_CANUSB_NO_MESSAGE == rv)
                {
                    // no messages, sleep
                    Thread.Sleep(1);
                    lock (this)
                    {
                        DataStructure.RXThreadTicks++;
                    }
                }

                else
                {
                    // error!
                    lock (this)
                    {
                        DataStructure.ReceiveErrors++;
                    }
                }
            }

            logFile.Close();
        }

        public void SendCANMessage(uint id, float testVar)
        {
            // Send one message with extended id
            LAWICEL.CANMsg msg = new LAWICEL.CANMsg();
            msg.id = id;
            msg.len = 8;
            msg.flags = LAWICEL.CANMSG_EXTENDED;
            byte[] data = new byte[8];
            BitConverter.GetBytes(testVar).CopyTo(data, 0);
            msg.data = BitConverter.ToUInt64(data, 0);
            int rv = LAWICEL.canusb_Write(canHandle, ref msg);
            if (LAWICEL.ERROR_CANUSB_OK == rv)
            {
                // OK
                lock (this)
                {
                    DataStructure.TransmitPackets++;
                }
            }
            else if (LAWICEL.ERROR_CANUSB_TX_FIFO_FULL == rv)
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
            else
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
        }

        string GlobalSerialBuffer = "";
        private void timerGUI_Tick(object sender, EventArgs e)
        {
            // update GUI
            // 0. Serial COM
            if( serialPort1.IsOpen)
            {
                try
                {
                    string read = serialPort1.ReadExisting();
                    if (read.Length > 0)
                    {
                        string toWrite = read;
                        toWrite = toWrite.Replace("\n\r", "\r\n");
                        File.AppendAllText(Application.StartupPath + "\\CommFile.txt", toWrite);

                        GlobalSerialBuffer += read; // cat
                        string[] tokens = GlobalSerialBuffer.Split('\r');
                        if (tokens.Length > 1)
                        {
                            string target = tokens[0];

                            // Do something
                            string[] splits = read.Split(',');
                            /*if (splits.Length == 13)
                            {
                                int amode = int.Parse(splits[1]);
                                float voltage = float.Parse(splits[2]);
                                int pwm1 = int.Parse(splits[3]);
                                int pwm2 = int.Parse(splits[4]);
                                int pwm3 = int.Parse(splits[5]);
                                int pwm4 = int.Parse(splits[6]);
                                float roll = float.Parse(splits[7]);
                                float pitch = float.Parse(splits[8]);
                                float yaw = float.Parse(splits[9]);
                                float gyroX = float.Parse(splits[10]);
                                float gyroY = float.Parse(splits[11]);
                                float gyroZ = float.Parse(splits[12]);

                                DataStructure.ModeActual = amode;
                                DataStructure.Voltage = voltage;

                                DataStructure.PWMOut1 = pwm1;
                                DataStructure.PWMOut2 = pwm2;
                                DataStructure.PWMOut3 = pwm3;
                                DataStructure.PWMOut4 = pwm4;
                            
                                DataStructure.Roll = roll;
                                DataStructure.Pitch = pitch;
                                DataStructure.Yaw = yaw;
                                DataStructure.GyroX = gyroX;
                                DataStructure.GyroY = gyroY;
                                DataStructure.GyroZ = gyroZ;
                            }
                            */
                            //sprintf(toSend, "$DATA,%d,%0.2f,%0.2f,%0.2f,%0.2f,%0.0f,%0.0f,%d,%0.8lf,%0.8lf\n\r", (int)df.ActualMode, df.BattVoltage, df.Roll, df.Pitch, df.Yaw, df.SonicCM, df.AbsPressurePa, (int)df.gpssatCount, df.gpsLongitude, df.gpsLatitude);
                            //%d,%0.8lf,%0.8lf\n\r", (int)df.ActualMode, df.BattVoltage, df.Roll, df.Pitch, df.Yaw, df.SonicCM, df.AbsPressurePa, (int)df.gpssatCount, df.gpsLongitude, df.gpsLatitude);
                            if (splits.Length == 12)
                            {
                                int amode = int.Parse(splits[1]);
                                float voltage = float.Parse(splits[2]);
                                float roll = float.Parse(splits[3]);
                                float pitch = float.Parse(splits[4]);
                                float yaw = float.Parse(splits[5]);
                                float sonicCM = float.Parse(splits[6]);
                                float AltF = float.Parse(splits[7]);
                                float VertSpeed = float.Parse(splits[8]);
                                int gpsSatCount = int.Parse(splits[9]);
                                double gpsLongitude = double.Parse(splits[10]);
                                double gpsLatitude = double.Parse(splits[11]);

                                DataStructure.ModeActual = amode;
                                DataStructure.Voltage = voltage;
                                DataStructure.Roll = roll;
                                DataStructure.Pitch = pitch;
                                DataStructure.MagYaw = yaw;
                                DataStructure.Sonar = sonicCM;
                                DataStructure.AltitudeF = AltF;
                                DataStructure.VertSpeed = VertSpeed;
                                DataStructure.GPSSatNr = gpsSatCount;
                                DataStructure.GPSLatitude = gpsLatitude;
                                DataStructure.GPSLongitude = gpsLongitude;

                            }
                            textBoxRXBox.Text = target;
                            //Debug.Write("X:" + target);
                        }
                        GlobalSerialBuffer = tokens.Last();
                    }
                }
                catch
                {
                }
            }

            // 1. get copy
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }

            // 2. set values
            textBoxTXRXFrames.Text = string.Format("{0}/{1}", copyStruct.TransmitPackets, copyStruct.ReceivedPackets);
            textBoxTXRXErrors.Text = string.Format("{0}/{1}", copyStruct.TransmitErrors, copyStruct.ReceiveErrors);
            textBoxRXTicks.Text = copyStruct.RXThreadTicks.ToString();


            textBoxModeActual.Text = string.Format("{0}", copyStruct.ModeActual);
            textBoxBattVoltage.Text = string.Format("{0:f1} V", copyStruct.Voltage);
            textBoxPWM1.Text = string.Format("{0:f1}", copyStruct.PWMOut1);
            textBoxPWM2.Text = string.Format("{0:f1}", copyStruct.PWMOut2);
            textBoxPWM3.Text = string.Format("{0:f1}", copyStruct.PWMOut3);
            textBoxPWM4.Text = string.Format("{0:f1}", copyStruct.PWMOut4);
            
            textBoxSonarM.Text = string.Format("{0:f2}", copyStruct.Sonar/100);
            textBoxIRFar.Text = string.Format("{0:f2}", copyStruct.IRFar/100);
            
            textBoxLoopMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTime);
            textBoxLoopMAXMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTimeMAX);
            textBoxTotalTime.Text = string.Format("{0:f2} s", copyStruct.TotalTime);

            textBoxMagX.Text = string.Format("{0:f3}", copyStruct.MagX);
            textBoxMagY.Text = string.Format("{0:f3}", copyStruct.MagY);
            textBoxMagZ.Text = string.Format("{0:f3}", copyStruct.MagZ);
            textBoxMagYaw.Text = string.Format("{0:f3}", copyStruct.MagYaw * 180 / Math.PI);
            textBoxdYaw.Text = string.Format("{0:f1} deg/s", copyStruct.GyroZ / Math.PI * 180);
            textBoxYaw42.Text = string.Format("{0:f1} deg", copyStruct.Yaw / Math.PI * 180);

            textBoxGyroX.Text = string.Format("{0:f2} °/s", copyStruct.GyroX * 180 / Math.PI);
            textBoxGyroY.Text = string.Format("{0:f2} °/s", copyStruct.GyroY * 180 / Math.PI);
            textBoxGyroZ.Text = string.Format("{0:f2} °/s", copyStruct.GyroZ * 180 / Math.PI);

            textBoxRoll.Text = string.Format("{0:f2} °", copyStruct.Roll * 180 / Math.PI);
            textBoxPitch.Text = string.Format("{0:f2} °", copyStruct.Pitch * 180 / Math.PI);
            textBoxYaw.Text = string.Format("{0:f2} °", copyStruct.Yaw * 180 / Math.PI);

            textBoxPressureABS.Text = string.Format("{0:f3} kPa", copyStruct.AbsPressure / 1000);
            textBoxPressureDiff.Text = string.Format("{0:f0} Pa", copyStruct.DiffPressure);
            textBoxTemperature.Text = string.Format("{0:f2} °C", copyStruct.IMUTemperature);
            double baroAltitude = 44330 * (1 - Math.Pow((copyStruct.AbsPressure / 101325), 1 / 5.255));
            textBoxAltitudeBaro.Text = string.Format("{0:f2} m", baroAltitude);
            double pitotSpeed = Math.Sqrt(2 * Math.Abs(copyStruct.DiffPressure) / 1.22521);
            if (copyStruct.DiffPressure < 0)
            {
                pitotSpeed = -pitotSpeed;
            }
            textBoxAirSpeed.Text = string.Format("{0:f2} kph", pitotSpeed * 3.6);
            textBoxGPSSatNr.Text = string.Format("{0:f1} ", copyStruct.GPSSatNr);
            if (copyStruct.GPSSatNr < 3) textBoxGPSSatNr.BackColor = Color.Red;
            else if (copyStruct.GPSSatNr < 4) textBoxGPSSatNr.BackColor = Color.Yellow;
            else textBoxGPSSatNr.BackColor = Color.Green;
            textBoxLongitude.Text = string.Format("{0:f8} °", copyStruct.GPSLongitude);
            textBoxLatitude.Text = string.Format("{0:f8} °", copyStruct.GPSLatitude);
            textBoxAltitudeGPS.Text = string.Format("{0:f2} m", copyStruct.GPSAltitude);
            textBoxVelX.Text = string.Format("{0:f2} m/s", copyStruct.velX);
            textBoxVelY.Text = string.Format("{0:f2} m/s", copyStruct.velY);

            textBoxAltitudeF.Text = string.Format("{0:f2} m", copyStruct.AltitudeF);
            textBoxVertSpeed.Text = string.Format("{0:f2} m", copyStruct.VertSpeed);

            // offsets
            textBoxRollOff.Text = string.Format("{0:f2} °", GRollOffset * 180 / Math.PI);
            textBoxPitchOff.Text = string.Format("{0:f2} °", GPitchOffset * 180 / Math.PI);

            // update mode
            int mode = (int)copyStruct.ModeActual;
            switch (mode)
            {
                case 0: //off
                    pictureOff.BackColor = Color.Lime;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = true;
                    buttonRollPitchMode.Enabled = true;
                    buttonAltMode.Enabled = true;
                    break;

                case 1: // RateMode
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Lime;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = false;
                    buttonRollPitchMode.Enabled = false;
                    buttonAltMode.Enabled = false;
                    break;

                case 2: // RollPitch Mode
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Lime;
                    pictureAltMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = false;
                    buttonRollPitchMode.Enabled = false;
                    buttonAltMode.Enabled = false;
                    break;

                case 3: // Alt Mode
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Lime;
                    buttonOff.Enabled = true;
                    buttonRateMode.Enabled = false;
                    buttonRollPitchMode.Enabled = false;
                    buttonAltMode.Enabled = false;
                    break;

                default:
                    pictureOff.BackColor = Color.Red;
                    pictureRate.BackColor = Color.Red;
                    pictureRollPitch.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    break;
            }

            // update status
            if( copyStruct.Fail == 0)
            {
                // OK
                pictureStatus.BackColor = Color.Green;
                buttonAckError.Enabled = false;
            }
            else
            {
                // FAIL
                pictureStatus.BackColor = Color.Red;
                buttonAckError.Enabled = true;
            }

            // draw waveforms
            waveForm1.Draw(panelWaveform1);
            waveForm2.Draw(panelWaveform2);

            // 3. read inputs (joy), TODO!!!
            JoyState js =joy.Poll();
            if (js == null) return;

            float thrRef = (float)js.Throttle;
            float rollRef = (float)js.AxisX;
            float pitchRef = (float)-js.AxisY;
            float yawRef = (float)js.AxisZ;

            // deadzone
            if (Math.Abs(rollRef) < 0.2) rollRef = 0;
            if (Math.Abs(pitchRef) < 0.2) pitchRef = 0;
            if (Math.Abs(yawRef) < 0.2) yawRef = 0;

            // scale
            rollRef = rollRef / 3;
            pitchRef = pitchRef / 3;

            // ADD offsets
            if (js.Hat1 == 90) GRollOffset += 0.0002;
            if (js.Hat1 == 270) GRollOffset -= 0.0002;
            if (js.Hat1 == 0) GPitchOffset -= 0.0002;
            if (js.Hat1 == 180) GPitchOffset += 0.0002;
            rollRef += (float)GRollOffset*2;
            pitchRef += (float)GPitchOffset*2;

            //if (mode >= 3) // altitude mode
            //{
            //   textBoxThrottleSet.Text = string.Format("{0:0.00 m}", (thrRef-0.1)/0.9 * 3);
            //}
            //else
            {
                textBoxThrottleSet.Text = string.Format("{0:0.00}", thrRef);
            }
            textBoxRollRef.Text = string.Format("{0:0.00}", rollRef);
            textBoxPitchRef.Text = string.Format("{0:0.00}", pitchRef);
            textBoxYawRef.Text = string.Format("{0:0.00}", yawRef);

            

            // MANUAL MODE
            /*
            double PWMSet = js.Throttle * 1000.0 + 1000;
            double Pitch = -js.AxisY * 100;
            double Roll = js.AxisX * 100;
            double Yaw = js.AxisZ * 50;

            double PWM1 = PWMSet;
            double PWM2 = PWMSet;
            double PWM3 = PWMSet;
            double PWM4 = PWMSet;
            // Pitch
            PWM4 += Pitch;
            PWM2 -= Pitch;

            // Roll
            PWM3 += Roll;
            PWM1 -= Roll;
            // Yaw
            PWM1 += Yaw;
            PWM3 += Yaw;
            PWM2 -= Yaw;
            PWM4 -= Yaw;
            if (PWM1 < 1000) PWM1 = 1000;
            if (PWM2 < 1000) PWM2 = 1000;
            if (PWM3 < 1000) PWM3 = 1000;
            if (PWM4 < 1000) PWM4 = 1000;
            if (PWM1 > 2000) PWM1 = 2000;
            if (PWM2 > 2000) PWM2 = 2000;
            if (PWM3 > 2000) PWM3 = 2000;
            if (PWM4 > 2000) PWM4 = 2000;
            textBoxPWM1.Text = string.Format("{0:0}", PWM1);
            textBoxPWM2.Text = string.Format("{0:0}", PWM2);
            textBoxPWM3.Text = string.Format("{0:0}", PWM3);
            textBoxPWM4.Text = string.Format("{0:0}", PWM4);
            */
             
            // 4. send messages
            bool silentMode = false;
            if (!silentMode)
            {
                uint baseAddr = XBEEBaseAddr;
                SendCANMessage(0x0110 + baseAddr, (float)rollRef);
                SendCANMessage(0x0111 + baseAddr, (float)pitchRef);
                SendCANMessage(0x0112 + baseAddr, (float)yawRef);
                SendCANMessage(0x0113 + baseAddr, (float)thrRef);

                GlobalRollRef = rollRef*0.5;

                //uint baseAddr = IOBaseAddr;
                //SendCANMessage(0x120 + baseAddr, (float)PWM1);
                //SendCANMessage(0x121 + baseAddr, (float)PWM2);
                //SendCANMessage(0x122 + baseAddr, (float)PWM3);
                //SendCANMessage(0x123 + baseAddr, (float)PWM4);

                // serial comm.
                if (serialPort1.IsOpen)
                {
                    string toSend = "$REF,";
                    toSend += string.Format("{0:0.000},", rollRef);
                    toSend += string.Format("{0:0.000},", pitchRef);
                    toSend += string.Format("{0:0.000},", yawRef);
                    toSend += string.Format("{0:0.000},", thrRef);
                    toSend += "\r";
                    serialPort1.Write(toSend);
                }
            }

            // 5. Read status (reset error???)
            //int bla = LAWICEL.canusb_Status(canHandle);

            // 6.draw Map 
            Graphics g = Graphics.FromImage(offscreenSurf);
            g.DrawImageUnscaled(bitMapMap, 0, 0);

            // Draw position!
            double LLx = copyStruct.GPSLongitude;
            double LLy = copyStruct.GPSLatitude;

            double PX = (LLx - 16.0721851) / 5.36800309E-06;
            double PY = (LLy - 45.80645595) / -3.82654066E-06;

            // direction
            double angle = copyStruct.MagYaw; // rads
            double AX = Math.Sin(angle) * 25;
            double AY = -Math.Cos(angle) * 25;
            
            if (Math.Abs(PX) < 1000 && Math.Abs(PY) < 1000)
            {
                g.FillEllipse(Brushes.Red, (float)PX - 5, (float)PY - 5, 10, 10);
                g.DrawLine(Pens.DarkCyan, (float)PX, (float)PY, (float)(PX + AX), (float)(PY + AY));
            }

            

            // copy to image
            Graphics targetG = panelMap.CreateGraphics();
            targetG.DrawImage(offscreenSurf, 0, 0);
        }

        private void buttonModeSelect_Click(object sender, EventArgs e)
        {
            uint modeAddress = XBEEBaseAddr + 0x0100;

            int mode = 0;

            if (sender == buttonOff) mode = 0;
            else if (sender == buttonRateMode) mode = 1;
            else if (sender == buttonRollPitchMode) mode = 2;
            else if (sender == buttonAltMode) mode = 3;

            if (serialPort1.IsOpen)
            {
                string toSend = "$MODE,";
                toSend += string.Format("{0:0},", mode);
                toSend += "\r";
                serialPort1.Write(toSend);
            }
            else
            {
                SendCANMessage(modeAddress, mode);
            }
        }     

        private void buttonAckError_Click(object sender, EventArgs e)
        {
            uint ackErrorAddress = XBEEBaseAddr + 0x0101;
            SendCANMessage(ackErrorAddress, 1);
        }

        private void buttonOpenCOM_Click(object sender, EventArgs e)
        {
            try
            {
                serialPort1.PortName = (string)comboBoxPorts.SelectedItem;
                serialPort1.Open();
                buttonOpenCOM.Enabled = false;
                buttonOpenCOM.Text = "Close";
                timerGUI.Enabled = true;
            }
            catch
            {
            }
        }

        private void buttonZeroOffsets_Click(object sender, EventArgs e)
        {
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }
            GRollOffset = copyStruct.Roll;
            GPitchOffset = copyStruct.Pitch;
        }
    }
}
